classdef singleJoint <handle
    %这个类是用于处理第三章的题目，没有考虑电机，只针对关节部分纯分析，所以也不涉及减速比
    %默认添加重力，不要应该手动disable
    properties
        m   = 0.5;         % mass of robot
        n   = 10;
        Ir   = 1.19e-5;        % MOI of motor ,I1自己算
        I1 = 0;
        l   = 0.1;         % length of robot
        Im;   %等效Im
        g   = 9.81;        % Gravity Constant
        B1  = 2e-2;         % 阻尼系数
        Br  = 4.1e-4; 
        Bm;   %等效阻尼系数
        
        %以顺时针为正方向
        is_g_enable = 1;    %0 for no applying
        state;  % pos& vel ,二维，行，可以有     多行，表示不同组的输入       
        n_group = 1;
       
    end
    
    methods
        function obj = singleJoint(origin_state,n,is_g_enable,m,I1,l,B1,Br)
            if nargin == 8
                obj.m = m;
                obj.n = n;
                obj.I1 = I1;
                obj.l = l;
                obj.B1 = B1;
                obj.Br = Br;
                obj.is_g_enable = is_g_enable;
                obj.state = origin_state;  
            elseif nargin == 1
                obj.state = origin_state;
                disp("only update origin state,g is enabled");
            else
                ME = MException('need at least 1 param for initial state');
                throw(ME)
            end
            obj.n_group = size(origin_state,1);
            if(obj.is_g_enable == 1)
                obj.I1 = (obj.m*obj.l^2);
                obj.Im = obj.Ir+ obj.I1/(obj.n^2); 
                obj.Bm = obj.Br+ obj.B1/(obj.n^2);
            else
                obj.Im = obj.Ir;
                obj.Bm = obj.Br;
            end
        end
        

        
        function state_info = simu_loop(obj,input_tau,simu_period)          
            %这里的state-info是三维的
            
            %因为从竖直位置开始记录theta，所以sin正好可以决定
            %重力矩的正负，所以没必要加这些，-bw就可以保证与速度方向相反
%             if obj.state(1)>pi
%                 g_factor = -1;
%             else
%                 g_factor = 1;
%             end
            
%             if(obj.state(2) >0)
%                 b_factor = -1;
%             else
%                 b_factor = 1;
%             end
                     
           ddot_theta = zeros(obj.n_group,1);
           for i = 1:obj.n_group
                if (obj.is_g_enable == 1)
                        ddot_theta(i) = (input_tau(i)+ obj.m*obj.g*obj.l*sin(obj.state(i,1))- obj.Bm*obj.state(i,2))/obj.Im;
                else
                        ddot_theta(i) = (input_tau(i)-obj.Bm*obj.state(i,2))/obj.Im;
                end
           end
                obj.state(:,2) = obj.state(:,2)+ ddot_theta*simu_period;
                obj.state(:,1) = obj.state(:,1)+ obj.state(:,2)*simu_period;
%                 if (find(obj.state(:,1)< 0,1))
%                     obj.state(i,1) = mod(obj.state(i,1),2*pi);
%                 end
%                 negative_rows = find(obj.state(:,1)< 0,obj.n_group);
%                 obj.state(negative_rows,1) = mod(obj.state(negative_rows,1),2*pi);
                % 没必要限定范围
            % not using    obj.state(1) >2*pi ||
%             disp(size(obj.state));
            state_info = [obj.state,ddot_theta]; 
            
        end
        
        
        
        function state_info = pos_step_control(obj,kp,ki,kd,sample_period,simu_step,simu_period)
            %这个是位置阶跃相应的一系列操作
            target_theta = pi/2-1*pi/180;
            pos_pid = pidControler(obj.n_group,kp,ki,kd,target_theta);
            disp(pos_pid.ref_value);
            disp(pos_pid.kp);
            disp(pos_pid.ki);
            disp(pos_pid.kd);
       
            state_info = zeros(obj.n_group,length(1:simu_step),3);
            debug_info = zeros(obj.n_group,length(1:simu_step),4);
            %pos_pid.get_add_up_con_value(obj.state(1))
            con_value = 0;
            sample_time_count = 0;
            for i = 1:simu_step
                state_info(:,i,:) = (obj.simu_loop(con_value,simu_period)).';
                sample_time_count = sample_time_count+simu_period;
                if(sample_time_count >= sample_period)
%                     con_value = pos_pid.get_sum_up_con_value(obj.state(1));
                    debug_info(:,i,:) = pos_pid.get_sum_up_con_value(obj.state(:,1));
                    con_value = debug_info(:,i,1);
                    sample_time_count = sample_time_count-sample_period;
                end
                
 


            %debug看是不是类的问题，不是！是目标位置设置不对，就89°即可
%             ref_value = pi/2-1*pi/180;
%             simu_period = 1e-5;                        
%             sample_period = 1e-3;
% 
%             state_info = zeros(length(1:simu_step),3);
%             debug_info = zeros(length(1:simu_step),4);
%             con_value = 0;
%             sample_time_count = 0;
%             err_sum = 0;
%             err_last = 0;
%             err_now = 0;
%             for i = 1:simu_step
%                 
%                 state_info(i,:) = (obj.simu_loop(con_value,simu_period)).';
%                 sample_time_count = sample_time_count+simu_period;
%                 if(sample_time_count >= sample_period)
%                     err_now = ref_value - obj.state(1);
%                     if(err_now < -pi)
%                         err_now = mod(err_now,pi);
%                     elseif(err_now > pi)
%                         err_now = mod(err_now,-pi);
%                     end
%                     err_sum = err_sum + err_now;
%                     con_value = kp * err_now + ki * err_sum + kd * (err_now - err_last);
%                     debug_info(i,3) =  err_last;
%                     err_last = err_now;
%                     debug_info(i,1) = con_value;
%                     debug_info(i,2) = err_now;
%                     debug_info(i,4) = err_sum;
%                     sample_time_count = sample_time_count-sample_period;
%                 end
%                 
% 


            end    
%             figure;
%             plot(1:simu_step,debug_info(:,2));
%             hold on
%             plot(1:simu_step,debug_info(:,3));
%             legend('err_now','err_last')
%             figure;
%             plot(1:simu_step,debug_info(:,1),'Color','g');
%             title('con')
            
            
        end
        
            
        function state_info = pos_traj_control(obj,kp,ki,kd,sample_period,simu_step,simu_period,target_traj)
            pos_pid = pidControler(kp,ki,kd,target_traj(1));
            state_info = zeros(length(1:simu_step),3);
            debug_info = zeros(length(1:simu_step),4);
            con_value = 0;
            sample_time_count = 0;
            for i = 1:simu_step
                state_info(i,:) = (obj.simu_loop(con_value,simu_period)).';
                sample_time_count = sample_time_count+simu_period;
                if(sample_time_count >= sample_period)
                    pos_pid.set_target(target_traj(1,i));
                    debug_info(i,:) = pos_pid.get_sum_up_con_value(obj.state(1));
                    con_value = debug_info(i,1);
                    sample_time_count = sample_time_count-sample_period;
                end
    
            end    
        end
        
        

        
        
        function disable_g(obj,zero)
            obj.is_g_enable = zero;  
            obj.Im = obj.Ir;
            obj.Bm = obj.Br;
        end
        
    end
    

         

end